/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of 
*     its contributors may be used to endorse or promote products 
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Joel Cartwright
*
*********************************************************************/

#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <nav_msgs/Odometry.h>

namespace transport_test
{

class Sender : public nodelet::Nodelet
{
public:
	Sender() {}

private:
	virtual void onInit()
	{
		ros::NodeHandle& private_nh = getMTNodeHandle();

		std::vector<std::string> argv = getMyArgv();

		if (argv.size() != 3)  {
			NODELET_ERROR_STREAM("Missing arguments.");
			NODELET_ERROR_STREAM("Expected arguments: reuseMsg|newMsg numMsgsToSend sendRate");
			ros::shutdown();
			return;
		}

		std::string msgUse = argv.at(0);
		numMsgsToSend_ = atoi(argv.at(1).c_str());
		sendRate_ = atoi(argv.at(2).c_str());
		msgNum_ = 0;

		pub_ = private_nh.advertise<nav_msgs::Odometry>("/transport_test", 1);

		if (msgUse == "reuseMsg") {
			NODELET_INFO_STREAM("Reusing single msg member variable.");
			timer_ = private_nh.createTimer(ros::Duration(1 / sendRate_), &Sender::callbackReuseMsg, this);
		}
		else if (msgUse == "newMsg") {
			NODELET_INFO_STREAM("Creating new msg each time (for nodelets).");
			timer_ = private_nh.createTimer(ros::Duration(1 / sendRate_), &Sender::callbackNewMsg, this);
		}
		else {
			NODELET_ERROR_STREAM("Invalid msg use type: " << msgUse);
			ros::shutdown();
			return;
		}

		NODELET_INFO_STREAM("Num to send: " << numMsgsToSend_);
		NODELET_INFO_STREAM("Send rate: " << sendRate_);

		int preSendDelay = 1;
		preSendDelayCount_ = sendRate_ * preSendDelay;
		NODELET_INFO_STREAM("Waiting for " << preSendDelay << " seconds to allow subscriber to connect.");

	}

	void callbackReuseMsg(const ros::TimerEvent& e)
	{
		if (preSendDelayCount_) {
			--preSendDelayCount_;
			return;
		}

		if (msgNum_ >= numMsgsToSend_) {
			//NODELET_INFO_STREAM("Last message sent, requesting node shutdown.");
			ros::shutdown();
			return;
		}

		++msgNum_;
		// data is intentionally left uninitialised, so we're only seeing transfer CPU usage.

		//NODELET_INFO_STREAM("Publishing reused msg " << msgNum_ << " of " << numMsgsToSend_);

		msg_.pose.pose.position.x = sendRate_;
		msg_.pose.pose.position.y = numMsgsToSend_;
		msg_.pose.pose.position.z = msgNum_;

		msg_.header.stamp = ros::Time::now();

		pub_.publish(msg_);
	}

	void callbackNewMsg(const ros::TimerEvent& e)
	{
		if (preSendDelayCount_) {
			--preSendDelayCount_;
			return;
		}

		if (msgNum_ >= numMsgsToSend_) {
			//NODELET_INFO_STREAM("Last message sent, requesting node shutdown.");
			ros::shutdown();
			return;
		}

		++msgNum_;
		// data is intentionally left uninitialised, so we're only seeing transfer CPU usage.

		//NODELET_INFO_STREAM("Publishing new msg " << msgNum_ << " of " << numMsgsToSend_);

		nav_msgs::OdometryPtr msg = nav_msgs::OdometryPtr(new nav_msgs::Odometry);

		msg->pose.pose.position.x = sendRate_;
		msg->pose.pose.position.y = numMsgsToSend_;
		msg->pose.pose.position.z = msgNum_;

		msg->header.stamp = ros::Time::now();

		pub_.publish(msg);
	}

	int preSendDelayCount_;

	ros::Timer timer_;
	ros::Publisher pub_;

	int numMsgsToSend_;
	float sendRate_;
	int msgNum_;

	nav_msgs::Odometry msg_;
};

PLUGINLIB_DECLARE_CLASS(transport_test, Sender, transport_test::Sender, nodelet::Nodelet);
}
